#include <Uart1.h>
#include "main.h"

#include "main_me.h"
#include "uart_command.h"
#include "inv_mpu9250_stm32F405port.h"
#include <cstring>


uint8_t key_pressed = 0 ;
uint8_t locked=0 ;
float x,y,z;



I2C_HandleTypeDef *p_i2c1;
Uart1 *p_uart1_tool;

void run_init(I2C_HandleTypeDef *_p_i2c1,
		UART_HandleTypeDef *p_uart1 )
{
	p_i2c1 = _p_i2c1;

	// 陀螺仪初始化
	port_init(p_i2c1);

	p_uart1_tool =  new Uart1(p_uart1);



}
void run_once()
{
	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET);

}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if (GPIO_Pin == GPIO_PIN_5)
    {
    	gyro_data_ready_cb();

    }
    else if (GPIO_Pin == GPIO_PIN_1)
    {
    	key_pressed = 1 ;

		HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);

		//去抖动
		HAL_Delay(500);
    }
}


void generat_cmd_headtrack_euler_angle(uint8_t *buffer, float* data_array)
{

	// Angle Set | p_e_vel| p_e_an | Ed| Md
	buffer[0] = CMD_FLAG;
	// tag
	buffer[1] = CMD_HEADTRACK_EULER_ANGLE;
	// len = deviceid之后有多少字节
	buffer[2] = 0x0E;
	//val.seq
	buffer[3] = 0x01;
	// val.deviceid
	buffer[4] = MY_DEVICE_ID;

	// roll
	memcpy(&buffer[5], &data_array[0], sizeof(float));
	// pitch
	memcpy(&buffer[9], &data_array[1], sizeof(float));
	// yaw
	memcpy(&buffer[13], &data_array[2], sizeof(float));

}

void generat_cmd_headtrack_lock(uint8_t *buffer)
{

	// Angle Set | p_e_vel| p_e_an | Ed| Md
	buffer[0] = CMD_FLAG;
	// tag
	buffer[1] = CMD_HEADTRACK_LOCK;
	// len
	buffer[2] = 3;
	//val.seq
	buffer[3] = 0x01;
	// val.deviceid
	buffer[4] = MY_DEVICE_ID;

	// val.locked
	buffer[5] = (locked +1)%2;
	locked = (locked +1)%2 ;


}




void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == USART1)
    {
    	p_uart1_tool->dma_callback();
    }
    else if(huart->Instance == USART2)
    {

    }
}


void run_loop()
{
	// 要休息的，不然功耗会一直高
	HAL_Delay(300);
	float euler_angle[3];

	//处理锁定按键
	if (key_pressed == 1 )
	{
		//因为按键抖动对电路晃动，要等一下，让陀螺仪稳定。
		HAL_Delay(1000);
		key_pressed = 0;

		// 跟踪锁定,重置0度角
		port_euler_angle(euler_angle);
		x = euler_angle[0];
		y = euler_angle[1];
		z = euler_angle[2];


		uint8_t b2[6] = { 0 };
		//生成锁定消息，并置 Locked
		generat_cmd_headtrack_lock(b2);
		p_uart1_tool->UART_send_bytes(b2, 6);
	}

	if (locked == 1)
	{

		uint8_t b2[18] =
				{ 0 };
		// 每次计算出欧拉角
		port_euler_angle(euler_angle);

		//减去校准角
		euler_angle[0] = euler_angle[0] - x;
		euler_angle[1] = euler_angle[1] - y;
		euler_angle[2] = euler_angle[2] - z;

		generat_cmd_headtrack_euler_angle(b2, euler_angle);
		HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
		p_uart1_tool->UART_send_bytes(b2, 18);
		//HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);

	}

	//p_uart1_tool->loop_deal_rcv_buffer();




}
